/**
 * @file Sensor.c
 * @author 周晨阳 (2922462750@qq.com)
 * @brief 传感器处理,负责获取角度传感器的数据
 * @version 0.1
 * @date 2021-11-23
 * 
 * @copyright Copyright (c) 2021
 * 
 */

/** @addtogroup  角度传感器模块
 *@{
 */

#include "Sensor.h"
#include "ADC.h"
#include "DSP.h"
//校准用的数据结构，校准采用一次函数y=kx+b
typedef struct
{
    /* data */
    float _K;
    float _b;

} Calibration;
Calibration sensorCaliList[7] = {{1, 0}, {1, 0}, {1, 0}, {1, 0}, {1, 0}, {1, 0}};
/**
 * @brief :传感器初始化
 * @param {*}
 * @return {*}
 */
void sensorInit()
{
    u8 i = 0;
    ADC_Inilize(MY_ADC_CHANNEL_0, 1000);
    for (i = 0; i < 7; i++)
    {
        sensorCaliList[i]._K = 1;
        sensorCaliList[i]._b = 0;
    }
}
/**
 * @brief :校准传感器
 * @param {u8} sensor 传感器
 * @param {float} k k值
 * @param {float} b b值
 * @return {*}
 */
void calibrateSensors(u8 sensor, float k, float b)
{
    sensorCaliList[sensor]._K = k;
    sensorCaliList[sensor]._b = b;
}
///< 滤波个数
#define N 50
#define N2 20

/**
 * @brief :
 * @param {u8} whichAngle 返回哪一个传感器角度
 * @param {u8} isLast 返回最新值还是上一个值
 * @return {*}
 */
float getAngle(u8 whichAngle, u8 isLast)
{
    u16 angleData;
    static float presentAngle = 0, lastAngle = 0; //此函数还可以返回上一次的角度值，不需要读取时间 ，是静态的数据，因为需要保存

#ifndef USE_RAW
    float tempValue[N] = {0};
    float tempValue2[N2] = {0};
    u8 i = 0, j = 0;

#endif

    if (isLast == PRESENT_ANGLE) //返回当前结果
    {
#ifndef USE_RAW

        //加入2阶中位值平均滤波法
        for (j = 0; j < N2; j++)
        {
            for (i = 0; i < N; i++)
            {
                tempValue[i] = getInputVoltageByAngle(whichAngle);
            }
            tempValue2[j] = filter(tempValue, N, 3300, 0);
        }
        //angleData = sensorCaliList[whichAngle]._K * filter(tempValue, N, 3300, 0) + sensorCaliList[whichAngle]._b;
        angleData = filter(tempValue2, N2, 3300, 0);

#else
        angleData = getInputVoltageByAngle(whichAngle);
#endif
        presentAngle = angleData;
        lastAngle = presentAngle;
        return presentAngle;
    }
    else //返回上一次结果
    {
        return lastAngle;
    }
}

/**
 *@}
 */
